% fc_fixed_point_solve

function td_angle = fc_fixed_point_solve()


clear('all');
close('all');
addpath('../animations', '../controllers', '../dynamics', '../environments', '../library');

fp_options = optimset('Display','iter','TolFun',1e-15, 'TolX',1e-10);

tic
td_angle = fsolve(@fp_fun,  evalin('base','first_guess'), fp_options);
error = fp_fun(td_angle)
toc

assignin('base','td_angle',td_angle);

% anim = AnimateForceControlModelClass(sim.gnd, output);
% anim.animate;


%% Fixed Point function

    function error = fp_fun(x)
        
        phases = 3;
        output = cell(phases, 2);
        
        x0 = [ 0, evalin('base','x_dot'), evalin('base','y'), 0, 0, 0, x, 0, ForceControlModelClass.r0 * ForceControlModelClass.G / ForceControlModelClass.rl, 0, 0];
        
        % controller = ComControllerClass;
        controller = SimpleControllerClass;
        
        sim = ForceControlModelClass(x0, @flat_rigid_environment, controller);
        
        for i = 1 : phases
            [output{i, 1}, output{i, 2}] = sim.run();
        end
        
        error = [   output{1, 2}(1, sim.dxci) - output{3, 2}(end, sim.dxci);    % horizontal velocity
            output{1, 2}(1, sim.yci) - output{3, 2}(end, sim.yci)       ];      % COM height
        
        % anim = AnimateForceControlModelClass(sim.gnd, output);
        % anim.animate;
        
    end


end